#!/usr/bin/env python

import roslib; roslib.load_manifest('roscolbot')
import rospy

from rosqwerk.msg import MotorStates
from rosqwerk.msg import MotorsControl
from roscolbot.msg import NeckControl


# Id of neck motor
NECK_MOTOR_ID=0

# Execution states
class ExecutionState:
    IDLE,GOING_DOWN,GOING_UP = range(3)
    
class NeckService():
    """
    Listen for up/down orders for neck and move it accordingly.
    (the neck state is published by the joint states publisher)
    """
    def __init__(self):
        rospy.init_node('colbot_neck_control', anonymous=True)
        
        rospy.Subscriber('colbot/neck_control',NeckControl, self.readNeckDemand)
        rospy.Subscriber('qwerk/motor_states', MotorStates, self.readMotorsAndControl)
        self.motorsControlPublisher = rospy.Publisher("/qwerk/motors_control", MotorsControl)
                
        self.neckIsUpPrevious=False
        self.neckIsUpDemand=False
        self.executionState=ExecutionState.IDLE
        
        self.motorCtrlMsg=MotorsControl()
        for i in range(len(self.motorCtrlMsg.motorVelocities)):
            self.motorCtrlMsg.motorVelocities[i]=MotorsControl.INVALID_VALUE
        self.motorCtrlMsg.motorPositions[NECK_MOTOR_ID]=MotorsControl.INVALID_VALUE          
                                                                                                                     
    def readNeckDemand(self,neckControl):
        rospy.loginfo("Received neck control demand, neck up %s" % neckControl.neckIsUp)
        self.neckIsUpDemand=neckControl.neckIsUp

    def readMotorsAndControl(self,motorStates):
        # stop when the current drawn by the motor is too important:
        # this is the sign we have reached the limit. We don't really
        # care where we are, we must stop anyway. If all goes well, this is
        # either full up or full down.
        #if (self.executionState!=ExecutionState.IDLE):
        #    rospy.loginfo("Neck motor current is %s" % motorStates.motorCurrents[NECK_MOTOR_ID])
        #    rospy.loginfo("Neck motor pos is %d" % motorStates.motorPositions[NECK_MOTOR_ID])
        if ((motorStates.motorCurrents[NECK_MOTOR_ID]>MotorStates.MOTOR_STALL_CURRENT_LIMIT) and (not self.executionState==ExecutionState.IDLE)):
            self.executionState=ExecutionState.IDLE
            # stop:
            self.motorCtrlMsg.motorVelocities[NECK_MOTOR_ID]=0
            self.motorsControlPublisher.publish(self.motorCtrlMsg)
            rospy.loginfo("Sending motor control stop")
        
        if ((self.neckIsUpDemand) and (not self.neckIsUpPrevious) and (not self.executionState==ExecutionState.GOING_UP)):
            # start going up:            
            self.motorCtrlMsg.motorVelocities[NECK_MOTOR_ID]=-MotorStates.MAX_MOTOR_SPEED_REQUEST                
            self.motorsControlPublisher.publish(self.motorCtrlMsg)
            self.executionState=ExecutionState.GOING_UP
            self.neckIsUpPrevious=self.neckIsUpDemand
            rospy.loginfo("Sending motor control up")
        if ((not self.neckIsUpDemand) and (self.neckIsUpPrevious) and (not self.executionState==ExecutionState.GOING_DOWN)):
            # start going down:
            self.motorCtrlMsg.motorVelocities[NECK_MOTOR_ID]=MotorStates.MAX_MOTOR_SPEED_REQUEST
            self.motorsControlPublisher.publish(self.motorCtrlMsg)
            self.executionState=ExecutionState.GOING_DOWN
            self.neckIsUpPrevious=self.neckIsUpDemand
            rospy.loginfo("Sending motor control down")

        
if __name__ == '__main__':
    try:
        NeckService()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

